Skip to content

IncrementalFixedLagSmoother: keep the newest variables at the root (1.5-5x faster updates, 5-100x faster marginals with multi-variable states) - #2922

Merged
dellaert merged 2 commits into
borglab:developfrom
jlblancoc:feature/ifls-params-newest-keys-last
Nov 8, 2026
Merged

dellaert merged 2 commits into
borglab:developfrom
jlblancoc:feature/ifls-params-newest-keys-last

Conversation

@jlblancoc

Copy link
Copy Markdown
Member

Hi! I'm a Claude agent presenting this PR on behalf of @jlblancoc 🙂

This builds on #2920 (ColamdConstrained accepting any increasing group indices), which makes three constraint groups safe.

Problem

createOrderingConstraints() puts the marginalizable keys in group 0 and every other key in group 1, in the updates that marginalize. That loses ISAM2's own default of ordering the keys of the new factors last, and with several variables per time step, or variables kept for the whole run (calibration, frame alignments), CCOLAMD then leaves the newest states deep in the Bayes tree. Each update re-eliminates the long path from the new factors to the root, and each marginalCovariance() of the current state computes a long shortcut. The class documentation says the active states are placed in/near the root.

Change

  • In every update (also those that marginalize nothing, and those without new factors), the keys of the new factors and the keys at the current timestamp go in a last constraint group, so the newest states stay at the root. Updates that marginalize nothing only constrain those keys, so the constraints stay small.
  • IncrementalFixedLagSmootherParams: the iSAM2 parameters plus the smoother options, so later options need no new constructors. Its first option, newestKeysLast, is on by default; setting it to false gives the previous ordering.
    IncrementalFixedLagSmootherParams params;  // isam2.findUnusedFactorSlots = true, as before
    params.isam2.relinearizeThreshold = 0.01;
    params.newestKeysLast = false;  // only to get the previous ordering
    IncrementalFixedLagSmoother smoother(lag, params);
  • The existing constructor (smootherLag, ISAM2Params) is kept, with the default options. params() still returns the iSAM2 parameters; the new smootherParams() returns all of them.
  • Python wrapper: the struct, the new constructor and smootherParams().
  • Tests: Params (defaults of both constructors) and NewestKeysLast (two variables per step; the newest one stays at the root).
  • timing/timeIncrementalFixedLagSmoother.cpp: the numbers below, from one binary, with the option off and on.

Performance

Release, one pinned core (Ryzen 7 2700X), median of 3 runs, option off -> on.

Multi-variable problems

timeIncrementalFixedLagSmoother 3000 2,6,12,30, i.e. 300 s at 10 Hz, timed once marginalization starts:

  • imu: pose, velocity and bias per step, CombinedImuFactor, odometry BetweenFactor, GPS at 1 Hz.
  • frames: poses with odometry and GPS, plus two pose sensors (alternating) measuring in their own frames, whose poses are retained variables.
  • odom: poses with odometry and GPS only.
problem lag s variables newest depth update ms marginalCovariance(newest) ms
imu 2 63 4 -> 0 0.25 -> 0.17 (1.5x) 0.185 -> 0.041 (5x)
imu 6 183 8 -> 0 0.46 -> 0.29 (1.6x) 0.308 -> 0.042 (7x)
imu 12 363 20 -> 0 0.86 -> 0.42 (2.0x) 0.697 -> 0.043 (16x)
imu 30 903 56 -> 0 2.06 -> 0.73 (2.8x) 1.825 -> 0.046 (40x)
frames 2 23 18 -> 0 0.26 -> 0.10 (2.6x) 0.430 -> 0.024 (18x)
frames 6 63 49 -> 0 0.60 -> 0.15 (4.0x) 1.108 -> 0.024 (46x)
frames 12 123 85 -> 0 1.04 -> 0.21 (5.0x) 1.874 -> 0.025 (75x)
frames 30 303 122 -> 0 1.84 -> 0.35 (5.2x) 2.688 -> 0.025 (108x)
odom 2 21 0 -> 0 0.054 -> 0.054 (1.00x) 0.017 -> 0.017
odom 30 301 0 -> 0 0.162 -> 0.162 (1.00x) 0.018 -> 0.018
  • Re-eliminated variables per update at a 30 s lag: 212 -> 53 (imu) and 159 -> 29 (frames); unchanged for odom.
  • Accuracy: the same mean error against ground truth in every row. The final estimates differ by at most 5e-5 m (imu, 30 s lag; partial relinearization depends on the ordering), 2e-13 m for frames, and not at all for odom.
  • Unpinned with TBB threads: the same ratios (e.g. imu 30 s 3.0x, frames 30 s 5.3x, odom 30 s 0.99x).

GTSAM's own examples

Their loops copied unchanged, except for building the smoother through IncrementalFixedLagSmootherParams and timing the update() calls of each step:

example problem variables update ms per step results
examples/FixedLagSmootherExample.cpp Pose2 chain, two odometry factors per step, lag 2 s, 2 updates per step 9 0.119 -> 0.120 (0.99x) identical
examples/IncrementalFixedLagSmootherExample.cpp issue1452.txt (FusionPortable, FAST-LIO2 odometry), Pose3 chain, lag 1 s, 3 updates per step 11 0.044 -> 0.044 (0.99x) differ by 4e-18
python/gtsam/examples/slam/EKF_SLAM.ipynb, its lag 0.99 s (an EKF) Pose2 and 15 landmarks, odometry and bearing-range, 50 steps 6 0.070 -> 0.070 (1.00x) identical
same, lag 10 s 26 0.150 -> 0.144 (1.04x) differ by 5e-12
same, lag 50 s (nothing marginalized) 66 0.140 -> 0.141 (0.99x) identical

They are single pose chains or small 2D windows, where the newest pose already ends at the root: they neither gain nor lose. The gains come with several variables per step or variables kept for the whole run.

Downstream

The same rule in MOLA's multi-sensor state estimator (odometry sources in their own frames, IMU, GNSS; prototyped as a subclass on GTSAM 4.2) makes it 2.0x faster in a synthetic LiDAR + wheel odometry setup, and 1.5-1.7x faster per scan on two Oxford Spires sequences.

Notes

  • On by default, so existing users may get slightly different numbers (iSAM2's partial relinearization depends on the ordering). newestKeysLast = false restores the previous ordering exactly.
  • Source compatibility: every existing call compiles unchanged, except a literal IncrementalFixedLagSmoother(lag, {}), which becomes ambiguous (there is none in GTSAM).

… and add a parameters struct

createOrderingConstraints() constrained only the marginalizable keys, to be
eliminated first, and only in updates that marginalize. Every other key
shared one group, so ISAM2's own default of ordering the observed keys last
was lost: the newest variables could end up deep in the Bayes tree, and then
each update re-eliminates a long path and each marginal of the current state
computes a long shortcut. The class documentation says the active states are
placed in/near the root.

Now, in every update, the keys of the new factors and the keys at the
current timestamp are eliminated last. Three groups rely on ColamdConstrained
accepting any group indices.

IncrementalFixedLagSmootherParams holds the iSAM2 parameters and the
smoother options, so new options need no new constructors. Its option
newestKeysLast, on by default, can be set to false for the previous
ordering. The existing constructor is kept and uses the default options.
Times updates and marginals of the newest pose with and without
newestKeysLast, on three problems: odometry with GPS, the same with IMU
states, and two pose sensors measuring in their own retained frames.
jlblancoc added a commit to MOLAorg/mola_state_estimation that referenced this pull request Nov 8, 2026
4.2's marginalizeLeaves() only removes the marginalized keys at the front
of a clique's frontals, and 4.2's update() only re-eliminates the children
that depend on them, so a key could stay in the Bayes tree without a value
and make the next update throw. On random windows, that happened in 96 of
1000 graphs with 4.2's own update, in 138 with the newest-keys-last
ordering, and in none with this change.

Also, the docs no longer say that newer GTSAM versions order the newest keys
last: borglab/gtsam#2922 is in no release yet, so GTSAM 4.3.x keeps its own
ordering and the parameter has no effect there. The unit test now checks the
three-key guard of the third constraint group directly, and runs random
windows that left marginalized keys behind without this change.

@dellaert dellaert left a comment

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fantastic!

@dellaert
dellaert merged commit 2a99e63 into borglab:develop Nov 8, 2026
8 of 14 checks passed
PeterCalifano added a commit to PeterCalifano/gtsam that referenced this pull request Nov 16, 2026
- Bring in borglab develop at 8806b33, including the incremental
  fixed-lag ordering of borglab#2922 and borglab#2920 and the ISAM2 fixes of borglab#2825,
  borglab#2820 and borglab#2801.

- CombinedImuFactor: keep integrateMeasurementWithDynamics() on top of
  upstream's sparse propagateCombinedCovariance(). The returned dense
  transition and gyro noise map follow the same block structure, so
  F P F' + Q reproduces preintMeasCov for every backend.

- LeggedEstimator: drop use2ndOrderCoriolis with upstream and keep the
  includes both sides need.

- Take upstream's HandleEigen.cmake and wrap MATLAB generator; select
  the system Eigen explicitly in build_gtsam.sh instead.

- Keep our deletion of the packaging trigger workflow.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants